package nezumi;

import java.awt.Dimension;
import java.awt.Point;
import java.awt.geom.Point2D;
import java.io.IOException;
import java.util.LinkedList;
import java.util.logging.Logger;

import nezumi.gui.AGui;
import nezumi.gui.AgentGui;
import nezumi.gui.MapGui;
import nezumi.planning.Planner;
import nezumi.state.AgentState;
import nezumi.state.SensorState;
import nezumi.state.State;
import nezumi.state.TargetState;
import nezumi.util.AngleGeometry;

import org.xml.sax.SAXException;

import ciberIF.ciberIF;

/**
 * A simple CiberRato client.
 * 
 * @author rui
 * 
 */
public class Nezumi {

	private static final Logger logger = Logger.getLogger(Nezumi.class
			.getName());

	public final static ciberIF cif = new ciberIF();
	public static boolean led = false;
	protected final static State state = new State();
	protected boolean isDone = false;
	private static final float MAX_DEV = 0.15f;
	private DecisionState dstate = DecisionState.EXPLORE;

	enum DecisionState {
		EXPLORE() {

			@Override
			public void transition() {
				logger.warning("Exploring");
			}

			@Override
			public DecisionState nextState() {
				if (state.getTargetState().isTargetSeen() != TargetState.NO_TARGET) {
					GOTARGET.transition();
					return GOTARGET;
				} else
					return this;
			}

			@Override
			public void execute() {

				SensorState sstate = state.getSensorState();
				if (sstate.getSensor(SensorState.IR_FRT) > 3
						|| sstate.getSensor(SensorState.IR_LFT) > 5.5
						|| sstate.getSensor(SensorState.IR_RGT) > 5.5)
					driveMotors(0.1, -0.1);
				// keep following wall
				else if ((sstate.getSensor(SensorState.IR_RGT) > 2.0
						&& sstate.getSensor(SensorState.IR_FRT) < 1.0 && sstate
						.getSensor(SensorState.IR_LFT) < 1.0)
						|| (sstate.getSensor(SensorState.IR_LFT) > 2.0
								&& sstate.getSensor(SensorState.IR_FRT) < 1.0 && sstate
								.getSensor(SensorState.IR_RGT) < 1.0))
					driveMotors(0.15, 0.15);
				else if (sstate.getSensor(SensorState.IR_LFT) > 1.5
						&& sstate.getSensor(SensorState.IR_LFT) < 1.5)
					driveMotors(0.1, 0.0);
				else if (sstate.getSensor(SensorState.IR_RGT) > 1.5)
					driveMotors(0.1, 0.0);
				else
					driveMotors(0.15, 0.15);
			}
		},

		GOTARGET() {

			@Override
			public void transition() {
				// activate the target
				TargetState targetState = state.getTargetState();
				int idx = targetState.isTargetSeen();
				Point2D target = targetState.getLikelyTargets()[idx];
				Planner planner = state.getPlanner();
				planner.setTargetId(idx);
				planner.setCurrTarget(new Point((int) target.getX(),
						(int) target.getY()));

				logger.warning(String.format("Going for target %d", idx));
			}

			@Override
			public DecisionState nextState() {
				if (state.getTargetState().isAllVisited()) {
					GOHOME.transition();
					return GOHOME;
					// if done with the current target, go explore
				} else if (state.getTargetState().isVisited(
						state.getPlanner().getTargetId())) {
					logger.warning("Turning led off\n");
					cif.SetVisitingLed(false);
					EXPLORE.transition();
					return EXPLORE;
				} else
					return this;
			}

			@Override
			public void execute() {

				// never do crash...
				if (emergencyManeuver())
					return;

				LinkedList<Point> path = state.getPlanner().getPath();

				if (!path.isEmpty())
					moveAlongPath(path);
				else
					EXPLORE.execute();
			}
		},

		GOHOME() {

			@Override
			public void transition() {
				// set the target to be the starting position
				Point2D startPos = state.getAgentState().getStartPos();
				Planner planner = state.getPlanner();

				// cif.SetReturningLed(true);
				planner.setCurrTarget(new Point((int) startPos.getX(),
						(int) startPos.getY()));
				planner.setGoingHome(true);

				// no way we're taking riscs on this one
				// MapState.WALKABLE_THRESHOLD /= 2;

				logger.warning("Going home");
			}

			@Override
			public void execute() {

				// never do crash...
				if (emergencyManeuver())
					return;

				LinkedList<Point> path = state.getPlanner().getPath();

				Point2D currPos = state.getAgentState().getCurrPos();
				Point2D startPos = state.getAgentState().getStartPos();
				// are we at the end?
				if ((path.isEmpty()
						&& ((int) currPos.getX()) == ((int) startPos.getX()) && ((int) currPos
						.getY()) == ((int) startPos.getY()))
						|| cif.GetTime() > 1790)
					// finish
					cif.Finish();
				else if (path.isEmpty())
					EXPLORE.execute();
				else
					moveAlongPath(path);
			}
		};

		public abstract void execute();

		public abstract void transition();

		public DecisionState nextState() {
			return this;
		}

		private static boolean emergencyManeuver() {
			SensorState sstate = state.getSensorState();

			// let us decide if we need to go into emergency mode
			if (sstate.getSensor(SensorState.IR_FRT) > 3.5
					|| sstate.getSensor(SensorState.IR_LFT) > 5
					|| sstate.getSensor(SensorState.IR_RGT) > 5) {
				driveMotors(-0.15, -0.15);
				prunePath();
				return true;
			}
			return false;
		}

		private static void moveAlongPath(LinkedList<Point> path) {

			assert path != null && !path.isEmpty() : "path = " + path;

			// target point
			final Point target = path.peek();

			final AgentState agentState = state.getAgentState();
			final Point currPosition = agentState.getCurrPosInt();
			double currOrientation = agentState.getCurrOrientation();
			double idealAngle = Math.atan2(target.y - currPosition.y, target.x
					- currPosition.x);

			// power in
			double lPowIn = 0, rPowIn = 0;

			// if the angle is correct, move forward, otherwise rotate in the
			// right
			// direction
			if (Math.abs(AngleGeometry.makePositive(currOrientation)
					- AngleGeometry.makePositive(idealAngle)) < MAX_DEV) {

				// compute the mean power we must apply to the wheels in order
				// to
				// reach the target

				final double minMeanPower = minMeanPower(target);
				if (minMeanPower < Constants.MOTOR_MAX_POW && path.size() > 1) {
					final double admissibleVar = Constants.MOTOR_MAX_POW
							- minMeanPower;
					final Point nextTarget = path.get(1); // next to the
					// current
					// one
					final double desiredAngle = Math.atan2(nextTarget.y
							- currPosition.y, nextTarget.x - currPosition.x);
					final double desiredrot = AngleGeometry.angleDifference(
							currOrientation, desiredAngle);

					double lPowInForRotation = Constants.MOUSE_RADIUS
							* -desiredrot - agentState.getLPowOut();

					double rPowInForRotation = Constants.MOUSE_RADIUS
							* desiredrot - agentState.getRPowOut();

					// normalization
					lPowInForRotation = 2 * Math.abs(lPowInForRotation) > Math
							.signum(lPowInForRotation)
							* admissibleVar ? admissibleVar : lPowInForRotation;
					rPowInForRotation = 2 * Math.abs(rPowInForRotation) > admissibleVar ? Math
							.signum(rPowInForRotation)
							* admissibleVar
							: rPowInForRotation;

					// rotate only a little
					lPowInForRotation /= 10;
					rPowInForRotation /= 10;

					lPowIn = minMeanPower + lPowInForRotation;
					rPowIn = minMeanPower + rPowInForRotation;

				} else
					lPowIn = rPowIn = 0.1;

			} else { // compute the ideal rotation to apply

				double deltatheta = AngleGeometry.angleDifference(
						currOrientation, idealAngle);

				assert Math.abs(deltatheta) <= Math.PI : "deltatheta = "
						+ deltatheta * 180 / Math.PI;

				lPowIn = 0.85 * (2 * Constants.MOUSE_RADIUS * -deltatheta - agentState
						.getLPowOut());
				rPowIn = 0.85 * (2 * Constants.MOUSE_RADIUS * deltatheta - agentState
						.getRPowOut());
			}

			// apply the power to the wheels
			lPowIn = constrain(lPowIn, -Constants.MOTOR_MAX_POW,
					Constants.MOTOR_MAX_POW);
			rPowIn = constrain(rPowIn, -Constants.MOTOR_MAX_POW,
					Constants.MOTOR_MAX_POW);

			driveMotors(lPowIn, rPowIn);

			final Point newPos = agentState.getCurrPosInt();

			// if we moved successfully to the next cell, remove actually two
			// things
			// from the path
			if (newPos.equals(target))
				path.poll();
			else if (!path.isEmpty())
				prunePath();
		}

		private static void driveMotors(double lPowIn, double rPowIn) {

			lPowIn = lPowIn > 0.15 ? 0.15 : lPowIn;
			lPowIn = lPowIn < -0.15 ? -0.15 : lPowIn;
			rPowIn = rPowIn > 0.15 ? 0.15 : rPowIn;
			rPowIn = rPowIn < -0.15 ? -0.15 : rPowIn;

			assert lPowIn >= -0.15 && lPowIn <= 0.15 : "lPowIn" + lPowIn;
			assert rPowIn >= -0.15 && rPowIn <= 0.15 : "rPowIn" + rPowIn;

			cif.DriveMotors(lPowIn, rPowIn);

			final AgentState aState = state.getAgentState();

			aState.updateAgentState(lPowIn, rPowIn);
		}

		/**
		 * Throw away positions that are too far away from the mouse.
		 */
		private static void prunePath() {

			// maybe we have strayed over to some
			// other
			// cell, so
			// let us try to find the cell that is closest to position we ended
			// up in

			final Point currPos = state.getAgentState().getCurrPosInt();
			final LinkedList<Point> path = state.getPlanner().getPath();

			// cannot work with on an empty path, just return and forget
			if (path.isEmpty())
				return;

			// other
			// cell, so
			// let us try to find the cell that is closest to position we ended
			// up in
			double currDist = currPos.distance(path.get(0));
			for (int i = 1; i < path.size(); i++) {
				final double dist = currPos.distance(path.get(i));
				if (dist <= currDist) {
					path.remove(i - 1);
					currDist = dist;
				} else
					break;
			}
		}

		/**
		 * Computes the mean power we must apply to the wheels in order to reach
		 * our target.
		 * 
		 * @param target
		 * @return
		 */
		private static double minMeanPower(Point target) {

			final AgentState astate = state.getAgentState();
			final Point2D currPos = astate.getCurrPos();
			final double rot = astate.getCurrOrientation();

			final double deltax = (target.getX() - currPos.getX())
					* Constants.MAP_GRANULARITY;
			final double deltay = (target.getY() - currPos.getY())
					* Constants.MAP_GRANULARITY;
			final double costheta = Math.cos(rot);
			final double sintheta = Math.sin(rot);

			double lin = Double.MAX_VALUE;

			if (Math.abs(deltax) > Math.abs(deltay) && deltax != 0
					&& costheta != 0)
				lin = deltax / costheta;
			else if (deltay != 0 && sintheta != 0)
				lin = deltay / sintheta;
			else if (deltax == 0 && deltay == 0)
				lin = 0;

			return Math.abs(lin);
		}

		/**
		 * Make sure a value fits into the specified interval
		 * 
		 * @param value
		 * @param min
		 * @param max
		 * @return
		 */
		private static double constrain(double value, double min, double max) {
			if (value < min)
				return min;
			else if (value > max)
				return max;
			else
				return value;
		}
	}

	/**
	 * Basic empty constructor.
	 */
	public Nezumi() {
		super();
	}

	/**
	 * Reads a new values, decides what to do and sends the resulting action to
	 * the simulator.
	 */
	public void mainLoop() {

		while (!isDone) {
			processSensors();

			// if we are already started
			if (cif.GetStartButton()) {
				// step the decision machine
				this.dstate = dstate.nextState();
				this.dstate.execute();
				requestSensors();
			}
		}
	}

	private void processSensors() {
		// read them sensors
		cif.ReadSensors();

		SensorState sensorState = state.getSensorState();

		// temporary value holder
		double irLeft = -1, irRight = -1, irFront = -1;

		if (cif.IsObstacleReady(0)) {
			irFront = cif.GetObstacleSensor(0);

			// do we have another sensor to reduce the error?
			// another question is, will the error be reduced at all?
			// because the sum of two N(0,1) distributions is itself
			// a N(0,1) distribution
			if (cif.IsObstacleReady(3))
				irFront = (irFront + cif.GetObstacleSensor(1)) / 2;
		}
		if (cif.IsObstacleReady(1))
			irLeft = cif.GetObstacleSensor(1);
		if (cif.IsObstacleReady(2))
			irRight = cif.GetObstacleSensor(2);

		sensorState.batchUpdateIRSensors(irLeft, irFront, irRight);

		if (cif.IsGroundReady())
			sensorState.updateGroundSensor(cif.GetGroundSensor());

		// if available, use the compass to correct orientation errors
		if (cif.IsCompassReady()) {
			final double compassReading = cif.GetCompassSensor();
			sensorState.setSensor(SensorState.COMPASS, compassReading);
			state.getAgentState().correctOrientation(compassReading);
		}

		// set the beacon value
		for (int i = 0; i < Constants.N_TARGETS; i++)
			if (cif.IsBeaconReady(i))
				sensorState.updateBeaconSensor(cif.GetBeaconSensor(i), i);
	}

	/**
	 * Request sensors from the ciberRato interface. Because we can only get 4
	 * sensors at a time, we cycle between them to get all the information we
	 * need.
	 */
	private void requestSensors() {

		// infrared sensors are always requested
		cif.RequestIRSensor(0);
		cif.RequestIRSensor(1);
		cif.RequestIRSensor(2);

		int time = (int) cif.GetTime();
		switch (time % 4) {
		case 1:
			cif.RequestGroundSensor();
			break;
		case 2:
			// used to reduce the error
			cif.RequestCompassSensor();
			break;
		case 3:
			// use the 4th IR sensor to reduce the error
			cif.RequestIRSensor(3);
			break;
		default:
			cif.RequestBeaconSensor(time % Constants.N_TARGETS);
			break;
		}
	}

	/**
	 * Get the program started
	 * 
	 * @param args
	 */
	public static void main(String[] args) {

		String host = "localhost", robName = "Nezumi", lab = null, grid = null;
		boolean gui = false;
		int gridPos = 1;
		int argIdx;

		// parse command-line arguments
		try {
			argIdx = 0;
			while (argIdx < args.length) {
				if (args[argIdx].equals("-pos")) {
					if (args.length > argIdx + 1) {
						gridPos = Integer.valueOf(args[argIdx + 1]).intValue();
						argIdx += 2;
					}
				} else if (args[argIdx].equals("-robname")) {
					if (args.length > argIdx + 1) {
						robName = args[argIdx + 1];
						argIdx += 2;
					}
				} else if (args[argIdx].equals("-host")) {
					if (args.length > argIdx + 1) {
						host = args[argIdx + 1];
						argIdx += 2;
					}
				} else if (args[argIdx].equals("-lab")) {
					if (args.length > argIdx + 1) {
						lab = args[argIdx + 1];
						argIdx += 2;
					}
				} else if (args[argIdx].equals("-grid")) {
					if (args.length > argIdx + 1) {
						grid = args[argIdx + 1];
						argIdx += 2;
					}
				} else if (args[argIdx].equals("-gui")) {
					gui = true;
					argIdx += 1;
				} else
					throw new Exception();
			}
		} catch (Exception e) {
			printUsage();
			return;
		}

		// create client
		final Nezumi client = new Nezumi();

		if (lab != null && grid != null) {
			// load the maze from file
			try {
				state.getMapState().loadMapState(lab, grid);
				state.getTargetState().loadTargetState(lab, grid);
			} catch (SAXException e) {
				logger.severe(String.format(
						"Malformed labyrinth files %s and %s. Exception: %s",
						lab, grid, e.getMessage()));
			} catch (IOException e) {
				logger
						.severe(String
								.format(
										"Error opening labyrinth files %s and %s. Exception: %s",
										lab, grid, e.getMessage()));
			}
		}

		// launch GUI in a new thread
		if (gui) {

			final String mapGuiName = "Nezumi Map GUI";

			Thread mapThread = new Thread(mapGuiName) {
				public void run() {
					AGui g = new MapGui(mapGuiName, new Dimension(560, 280),
							state, 50);
					// into a never-ending cycle
					g.guiLoop();
				}
			};

			final String agentGuiName = "Nezumi Agent GUI";

			Thread agentThread = new Thread(agentGuiName) {
				public void run() {
					AGui g = new AgentGui(agentGuiName,
							new Dimension(440, 250), state, 1000);
					// into a never-ending cycle
					g.guiLoop();
				}
			};

			// both are low priority, but the agent thread has a little edge
			mapThread.setPriority(Thread.MIN_PRIORITY);
			agentThread.setPriority(Thread.MIN_PRIORITY + 1);

			// actually launch them threads
			mapThread.start();
			// agentThread.start();
		}

		final double[] sensorPositions = { Constants.IR_FRT_ANGLE,
				Constants.IR_LFT_ANGLE, Constants.IR_RGT_ANGLE,
				Constants.IR_RGT_ANGLE };

		// register robot in simulator
		cif.InitRobot2(robName, gridPos, sensorPositions, host);

		// main loop
		client.mainLoop();
	}

	/**
	 * Print help instructions.
	 */
	static void printUsage() {
		System.out.println("Usage: java Client [-robname <robname>] "
				+ "[-pos <pos>] [-host <hostname>[:<port>]]");
	}
}